#pragma once
#include "g2o/core/base_vertex.h"

namespace g2o
{
    class VertexQ : public g2o::BaseVertex<3, Eigen::Quaterniond>
    {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
        VertexQ();
        virtual bool read(std::istream &is);

        virtual bool write(std::ostream &os) const;

        virtual void setToOriginImpl() { _estimate = Eigen::Quaterniond(1, 0, 0, 0); }

        virtual void oplusImpl(const double *update_)
        {
            Eigen::Map<const Eigen::Vector3d> update(update_);
            Eigen::Quaterniond qq(1, update[0] / 2, update[1] / 2, update[2] / 2);
            qq.normalize();
            _estimate = _estimate * qq;
            _estimate.normalize();
        }

        virtual bool setEstimateDataImpl(const double *est)
        {
            Eigen::Map<const Eigen::Vector4d> _est(est);
            _estimate.coeffs() = _est;
            _estimate.normalize();
            return true;
        }

        virtual bool getEstimateData(double *est) const
        {
            Eigen::Map<Eigen::Vector4d> _est(est);
            _est = _estimate.coeffs();
            return true;
        }

        virtual int estimateDimension() const
        {
            return 4;
        }

        virtual bool setMinimalEstimateDataImpl(const double *est)
        {
            Eigen::Vector3d v = Eigen::Map<const Eigen::Vector3d>(est);
            double theta = v.norm();
            if (theta != 0)
            {
                v = v / theta;
                v = std::sin(theta / 2) * v;
            }
            _estimate = Eigen::Quaterniond(std::cos(theta / 2), v[0], v[1], v[2]);
            return true;
        }

        virtual bool getMinimalEstimateData(double *est) const
        {
            Eigen::Map<Eigen::Vector3d> v(est);
            double theta = std::acos(std::fabs(_estimate.w()));
            theta = 2 * theta;
            v[0] = _estimate.x();
            v[1] = _estimate.y();
            v[2] = _estimate.z();
            if (_estimate.w() < 0)
            {
                v = -v;
            }
            if (theta != 0 && v.norm() != 0)
            {
                v.normalize();
                v = v * theta;
            }
            else
            {
                v = Eigen::Vector3d(0, 0, 0);
            }
            return true;
        }

        virtual int minimalEstimateDimension() const
        {
            return 3;
        }
    };
} // namespace g2o